//------------------------------------------------------------------------------
//
//  Integrator Robot Brick
//
//  Written Jan-2008 by Trevor Taylor, Software Technology, Australia
//
//  This service is for the Integrator robot and is based on the Generic
//  Brick contact.
//
//  A generic differential drive service is also incorporated into this DLL.
//  There is no bumper service however because there is only one IR sensor
//  and it has a digital output.
//
//  NOTE: Although the Generic Brick contract has operations for
//  DriveDistance and RotateDegrees, they are not implemented here.
//  They are in the IntegratorDrive service. However, there is no reason
//  why they could not be moved here, and then IntegratorDrive would be
//  just a shell. This service controls the motors anyway.
//
//------------------------------------------------------------------------------

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using W3C.Soap;
using integrator = ProMRDS.Robotics.Integrator;


using submgr = Microsoft.Dss.Services.SubscriptionManager;

// Use the Generic Brick contract
using brick = ProMRDS.Robotics.GenericBrick.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;


// For HTTP stuff
using System.Net;
using Microsoft.Dss.Core.DsspHttp;

using util = ProMRDS.Robotics.GenericBrickUtil.Util;


using ProMRDS.Robotics.Integrator.Comms;


namespace ProMRDS.Robotics.Integrator
{
    /// <summary>
    /// Implementation class for Integrator Service
    /// </summary>
    // The control code uses a polling loop that blocks a thread.
    // The ActivationSettings attribute with Sharing = false makes the runtime
    // dedicate a dispatcher thread pool just for this service.
    [ActivationSettings(ShareDispatcher = false, ExecutionUnitsPerDispatcher = 4)]
    [DisplayName("Integrator")]
    [Description("Integrator Robot Service")]
    [Contract(Contract.Identifier)]
    [AlternateContract("http://www.promrds.com/contracts/2008/01/genericbrick.html")]
    public class IntegratorService : DsspServiceBase
    {
        /// <summary>
        /// State - Based on Generic Brick
        /// </summary>
        // Add an initial state partner
        [InitialStatePartner(Optional = true, ServiceUri = "Integrator.Config.xml")]
        private brick.GenericBrickState _state = new brick.GenericBrickState();

        private bool _newState = false;

        // Add an XSLT file to format the state
        // The filename is appended to the default namespace for this solution
        [EmbeddedResource("ProMRDS.Robotics.Integrator.Resources.Integrator.xslt")]
        string _transform = null;

        /// <summary>
        /// Main Port - Based on Generic Brick
        /// </summary>
        [ServicePort("/integrator", AllowMultipleInstances=true)]
        private brick.GenericBrickOperations _mainPort = new brick.GenericBrickOperations();

        // Subscription Manager Partner
        [Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        // Comms control
        private IntegratorControl _control = null;

        // Internal port used for polling
        Port<int> _internalPort = new Port<int>();

        // Minimum polling interval (in millisec) that is acceptable due to bandwidth, etc.
        private const int _minimumPollingInterval = 50;

        // Change this value for your setup or edit the config file
        private const int DefaultSerialPort = 7;

        private bool _shutdown = false;

        // Internal information about the LEDs on the robot
        private int[] LEDIds;
        private int LEDMask;


        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public IntegratorService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }

        #region Initialization

        /// <summary>
        /// Integrator Service Start
        /// </summary>
        protected override void Start()
        {
            // Make sure that the state is consistent
            InitializeState();
//            ProMRDS.Robotics.GenericBrickUtil.Resources res = new ProMRDS.Robotics.GenericBrickUtil.Resources();

            // Announce ourselves to the world
            base.Start();

            // Create a new controller
            _control = new IntegratorControl(_mainPort);

            // Try to connect to the robot
            // NOTE: If this fails, too bad! Stop the service and re-start it.
            if (ConnectToRobot())
            {
                // Kick off polling
                SetPollTimer();
            }

            // Save the state now because it contains the Firmware info.
            // Even if the robot failed to start, we might need to create an
            // empty config file for the user to edit.
            // Don't do this! For some reason it overwrites good files
            // with bad ones
            if (_newState)
                SaveState(_state);
        }


        /// <summary>
        /// Initialize/Validate State
        /// </summary>
        private void InitializeState()
        {
            if (_state == null)
            {
                _state = new brick.GenericBrickState();
                _newState = true;
                LogInfo(LogGroups.Console, "Configuration not found - Using defaults");
            }

            _state.Name = "RobotA Integrator Jan-08 V1.0";

            // Flag all of the capabilities of this brick
            // CanDriveDistance and CanRotateDegrees are not implemented here,
            // but the Drive service does implement them.
            _state.Capabilities =
                            brick.BrickCapabilities.CanControlMotors |
//                            brick.BrickCapabilities.CanDriveDistance |
//                            brick.BrickCapabilities.CanRotateDegrees |
                            brick.BrickCapabilities.HasDigitalIn |
                            brick.BrickCapabilities.HasDigitalOut |
                            brick.BrickCapabilities.HasDifferentialDrive |
                            brick.BrickCapabilities.HasBumpers |
                            brick.BrickCapabilities.HasBuzzer |
                            brick.BrickCapabilities.HasLEDs;

            // Reset these in case they were in the saved state
            _state.Connected = false;

            // Always reinitialize the Firmware or it might carry forward
            // the wrong information
            _state.Firmware = new brick.Firmware();

            if (_state.CommsStats == null)
                _state.CommsStats = new brick.CommsStats();

            _state.CommsStats.MessagesSent = 0;
            _state.CommsStats.MessagesReceived = 0;
            _state.CommsStats.ErrorCount = 0;

            if (_state.Configuration == null)
            {
                _state.Configuration = new brick.CommsConfig();
                _state.Configuration.SerialPort = DefaultSerialPort;
                _state.Configuration.BaudRate = 9600;
                _state.Configuration.Delay = 75;
                _state.Configuration.PollingInterval = _minimumPollingInterval;
                _state.Configuration.Timeout = 250;
            }
            // If PollingInterval is negative, then no polling will occur.
            // In this case, any partner must explicity read the sensors
            // by Get messages.
            if (_state.Configuration.PollingInterval >= 0 &&
                _state.Configuration.PollingInterval < _minimumPollingInterval)
                _state.Configuration.PollingInterval = _minimumPollingInterval;

            // Always initialize the Sensors just in case the
            // config file has been tampered with --
            // we don't support reconfiguration anyway
            InitializeSensors();

            // Always initialize the Actuators
            InitializeActuators();

            // Set up an array for easy access to the LEDs.
            // NOTE: There is no requirement for the LEDs to be in any
            // particular order. They should be in some logical order
            // that the user can understand, but this is not necessarily
            // the same as the numeric order of the Pins (or Hardware IDs).
            // The order in the array determines the bit positions in the
            // LEDs state property. This is a subtle relationship.
            LEDIds = new int[2];
            LEDIds[0] = (int)HwIds.LED1;
            LEDIds[1] = (int)HwIds.LED2;
            LEDMask = 0x03;

            if (_state.DrivePower == null)
                _state.DrivePower = new brick.DrivePower();

            // Initialize the Motors to stopped
            _state.DrivePower.LeftPower = 0;
            _state.DrivePower.RightPower = 0;

            
        }


        // Hardware numbering is as follows to make them unique:
        // Input Pins 0-7 = 1 - 8
        // Output Pins 0-7 = 11 - 18

        void InitializeSensors()
        {
            // Create a new Sensors list
            _state.Sensors = new List<brick.Device>();

            // Add the Sensor devices
            // NOTE: The order is not important, although it makes sense to put
            // the most frequently accessed sensors first.

            // Create the Analog Inputs --
            // Currently there are none but two input pins can be 
            // configured as ADCs if necessary: Pins 0 and 1

            // Create the Digital Inputs
            // NOTE: There are only 5 digital inputs on the PICAXE-18X,
            // so Pins 3, 4 and 5 are not defined because it is pointless
            // They are arranged in the order of frequency of use.
            _state.Sensors.Add(new brick.Device(
                "IR Bumper",
                (int)HwIds.IrBumper,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Bumper,
                brick.Location.FrontCenter,
                0,
                (int)Pins.IrBumper,     // IN6 = IR obstacle sensor
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Free",
                8,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Unused,
                brick.Location.Unknown,
                0,
                7,                      // IN7 is unused
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Free",
                1,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Unused,
                brick.Location.Unknown,
                0,
                0,                      // IN0 is Unused
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Free",
                2,
                brick.DeviceTypes.DigitalIn,
                brick.DeviceFunctions.Unused,
                brick.Location.Unknown,
                0,
                1,                      // IN1 is Unused
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Sensors.Add(new brick.Device(
                "Bluetooth In",
                3,
                brick.DeviceTypes.CommsIn | brick.DeviceTypes.Reserved,
                brick.DeviceFunctions.Comms,
                brick.Location.Other,
                0,
                2,                      // IN2 is Bluetooth - DO NOT USE
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );

        }

        void InitializeActuators()
        {
            // Create a new Actuators list
            _state.Actuators = new List<brick.Device>();

            // Add the Actuator devices
            // NOTE: The order is not important, although it makes sense to put
            // the most frequently accessed actuators first.

            // There are no Analog Outputs

            // Digital Outputs
            _state.Actuators.Add(new brick.Device(
                "LED 1",
                (int)HwIds.LED1,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.CenterLeft,
                0,
                (int)Pins.LED1,         // OUT5 is LED1
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "LED 2",
                (int)HwIds.LED2,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.LED,
                brick.Location.CenterLeft,
                0,
                (int)Pins.LED2,         // OUT6 is LED2
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Buzzer",
                (int)HwIds.Buzzer,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.Buzzer,
                brick.Location.Other,
                0,
                (int)Pins.Buzzer,       // OUT7 is Buzzer
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );

            // The remaining Pins should not be used directly
            _state.Actuators.Add(new brick.Device(
                "Left Motor Reverse",
                11,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.MotorDirection,
                brick.Location.SideLeft,
                0,
                0,                      // OUT0 is a Motor - DO NOT USE
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Left Motor Forward",
                12,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.MotorDirection,
                brick.Location.SideLeft,
                0,
                1,                      // OUT1 is a Motor - DO NOT USE
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Right Motor Reverse",
                13,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.MotorDirection,
                brick.Location.SideRight,
                0,
                2,                      // OUT2 is a Motor - DO NOT USE
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Right Motor Forward",
                14,
                brick.DeviceTypes.DigitalOut,
                brick.DeviceFunctions.MotorDirection,
                brick.Location.SideRight,
                0,
                3,                      // OUT3 is a Motor - DO NOT USE
                DateTime.Now,
                false,
                0,
                3,
                1
                )
            );
            _state.Actuators.Add(new brick.Device(
                "Bluetooth Out",
                15,
                brick.DeviceTypes.CommsOut | brick.DeviceTypes.Reserved,
                brick.DeviceFunctions.Comms,
                brick.Location.Other,
                0,
                4,                      // OUT4 is Bluetooth - DO NOT USE
                DateTime.Now,
                false,
                0,
                0,
                1
                )
            );
        }

        #endregion

        #region Operation Handlers

        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(brick.Get get)
        {
            // Make sure the comms stats are up to date
            _state.CommsStats.MessagesSent = _control.MessagesSentCounter;
            _state.CommsStats.ErrorCount = _control.ErrorCounter;

            get.ResponsePort.Post(_state);
            yield break;
        }

        /// <summary>
        /// HTTP Get Handler
        /// </summary>
        /// <param name="httpGet"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            // Make sure the comms stats are up to date
            _state.CommsStats.MessagesSent = _control.MessagesSentCounter;
            _state.CommsStats.ErrorCount = _control.ErrorCounter;

            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _state,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost submit)
        {
            throw new NotImplementedException("HttpPost is not implemented");
        }

        /// <summary>
        /// Replace Handler
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ReplaceHandler(brick.Replace replace)
        {
            _state = replace.Body;
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Subscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SubscribeHandler(brick.Subscribe subscribe)
        {
            //base.SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort);
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success) { },
                delegate(Exception ex)
                {
                    LogError(ex);
                    throw ex;
                }
            );
            yield break;
        }

        /// <summary>
        /// ConfigureBrick Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ConfigureBrickHandler(brick.ConfigureBrick update)
        {
            // Configuration options are very limited!
            if (update.Body.Delay >= 0)
            {
                _state.Configuration.Delay = update.Body.Delay;
                _control.Delay = _state.Configuration.Delay;
            }

            _state.Configuration.PollingInterval = update.Body.PollingInterval;

            // Timeout is set, but not acted on
            if (update.Body.Timeout >= 0)
                _state.Configuration.Timeout = update.Body.Timeout;

            // Save the state for next time
            SaveState(_state);
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// ConfigureDevices Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> ConfigureDevicesHandler(brick.ConfigureDevices update)
        {
            throw new NotImplementedException("ConfigureDevices is not implemented in version 1.0");
        }

        /// <summary>
        /// Update Sensors Handler (Selective Update)
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> UpdateSensorsHandler(brick.UpdateSensors update)
        {
            bool changed = false;
            int i;

            // Iterate through all of the Inputs in the request
            foreach (brick.Device d in update.Body.Devices)
            {
                // Find the matching Hardware Id
                // This is inefficient, but it keeps the structures simple
                i = util.FindDeviceById(_state.Sensors, d.HardwareIdentifer);
                if (i < 0)
                    throw new ArgumentOutOfRangeException("Invalid Hardware Identifier: " + d.HardwareIdentifer);

                // Update according to the device type
                // NOTE: If it is not Analog or Digital, it is ignored!
                if (util.IsAnalogDevice(d))
                {
                    util.UpdateDeviceValue(_state.Sensors[i], d.Value);
                    changed = true;
                }
                if (util.IsDigitalDevice(d))
                {
                    // Update the state
                    util.UpdateDeviceState(_state.Sensors[i], d.State);
                    changed = true;
                }
            }

            // Send Notifications to subscribers if changed
            // NOTE: There are two options here --
            // Send all the sensors and let subscribers figure out what has changed
            // or send only the changes that we just received. In keeping with the
            // minimalist approach, we just send the update.
            if (changed)
                SendNotification<brick.UpdateSensors>(_subMgrPort, update);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// PlayTone Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> PlayToneHandler(brick.PlayTone update)
        {
            // Turn on the buzzer and don't bother to wait for a response
            util.UpdateDeviceStateInList(_state.Actuators, (int)HwIds.Buzzer, true);
            _control.SetPin((int)Pins.Buzzer, true);

            // Set up a timer to turn the buzzer off
            Activate(Arbiter.Receive(false, TimeoutPort(update.Body.Duration),
                delegate(DateTime dt)
                {
                    // Turn off the buzzer
                    util.UpdateDeviceStateInList(_state.Actuators, (int)HwIds.Buzzer, false);
                    _control.SetPin((int)Pins.Buzzer, false);
                }
            ));

            // Send back the default response
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// SetLEDs Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetLEDsHandler(brick.SetLEDs update)
        {
            // Use the LEDMask to chop off invalid bits
            _state.LEDs = update.Body.States & LEDMask;

            // Update the Sensors list as well
            int mask = 1;
            for (int i = 0; i < LEDIds.Length; i++)
            {
                util.UpdateDeviceStateInList(_state.Actuators, LEDIds[i], ((mask & _state.LEDs) != 0));
                mask = mask << 1;
            }

            // Set the LEDs now
            _control.SetLEDs(_state.LEDs);

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// GetSwitches Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetSwitchesHandler(brick.GetSwitches query)
        {
            throw new InvalidOperationException("There are no Switches");
        }

        /// <summary>
        /// GetSensors Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetSensorsHandler(brick.GetSensors query)
        {
            int n;

            // Create a new result list (which might end up empty!)
            List<brick.Device> list = new List<brick.Device>();

            foreach (brick.Device d in query.Body.Inputs)
            {
                // Look in the State for a matching Hardware Id
                n = util.FindDeviceById(_state.Sensors, d.HardwareIdentifer);
                if (n < 0)
                {
                    // No such device! Throw an exception and give up!
                    throw new ArgumentOutOfRangeException("No such hardware identifier: " + d.HardwareIdentifer);
                }
                else
                {
                    // Add the device to the result list
                    list.Add((brick.Device)_state.Sensors[n].Clone());
                }
            }
            // Return the list
            query.ResponsePort.Post(new brick.GetSensorsResponse(list));

            yield break;
        }

        /// <summary>
        /// SetActuators Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        /// NOTE: Do NOT send commands to Motors or Reserved Pins this way!
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetActuatorsHandler(brick.SetActuators update)
        {
            foreach (brick.Device d in update.Body.Outputs)
            {
                // Only Digital devices are handled here
                if (util.IsDigitalDevice(d))
                {
                    if (d.Function == brick.DeviceFunctions.LED)
                    {
                        _state.LEDs = util.UpdateBitmask(_state.LEDs, LEDIds,
                                                    d.HardwareIdentifer, d.State);
                    }
                    // Now set the pin
                    util.UpdateDeviceStateInList(_state.Actuators, d.HardwareIdentifer, d.State);
                    _control.SetPin(d.Pin, d.State);
                }
            }
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// QueryDrivePower Handler
        /// </summary>
        /// <param name="query"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> QueryDrivePowerHandler(brick.QueryDrivePower query)
        {
            query.ResponsePort.Post(_state.DrivePower);
            yield break;
        }

        /// <summary>
        /// Set Drive Power Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            if (!_state.Connected)
            {
                throw new InvalidOperationException("Not connected to robot");
            }

            Port<drive.SetDrivePower> p = (Port<drive.SetDrivePower>)_mainPort[typeof(drive.SetDrivePower)];
            if (p.ItemCount > 10)
                LogError(LogGroups.Console, "SetDrivePower backlog: " + p.ItemCount);

            // Remove extra requests that might be banking up
            drive.SetDrivePower upd;
            int count = 0;
            while ((upd = _mainPort.Test<drive.SetDrivePower>()) != null)
            {
                // Acknowledge the update
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                count++;
                update = upd;
            }

            if (count > 1)
                LogError(LogGroups.Console, "Skipped drive updates: " + count);

            bool changed = ((update.Body.LeftWheelPower != null && 
                _state.DrivePower.LeftPower != update.Body.LeftWheelPower)
                || (update.Body.RightWheelPower != null &&
                _state.DrivePower.RightPower != update.Body.RightWheelPower));

            if (update.Body.LeftWheelPower != null &&
                update.Body.LeftWheelPower >= -1.0 &&
                update.Body.LeftWheelPower <= 1.0)
                _state.DrivePower.LeftPower = update.Body.LeftWheelPower;

            if (update.Body.RightWheelPower != null &&
                update.Body.RightWheelPower >= -1.0 &&
                update.Body.RightWheelPower <= 1.0)
                _state.DrivePower.RightPower = update.Body.RightWheelPower;

            // If we are connected, send the Power to the robot
            if (_state.Connected)
            {
                yield return Arbiter.Receive(false,
                    _control.SetPower(_state.DrivePower.LeftPower, _state.DrivePower.RightPower),
                    delegate (byte[] resp)
                    { }
                );

                // Send Notifications to subscribers
                if (changed)
                {
                    //_subMgrPort.Post(new submgr.Submit(_state.DrivePower, DsspActions.UpdateRequest));
                    SendNotification<drive.SetDrivePower>(_subMgrPort, new drive.SetDrivePowerRequest(_state.DrivePower.LeftPower, _state.DrivePower.RightPower));
                }
            }
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            throw new NotImplementedException("DriveDistance is not implemented - Use Drive service");
        }

        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            throw new NotImplementedException("RotateDegrees is not implemented - Use Drive service");
        }

        #endregion

        #region Polling

        /// <summary>
        /// Set a Timer for Polling
        /// </summary>
        // NOTE: This does NOT guarantee an exact polling interval.
        // Instead it adds the polling interval to the current time.
        // There is only ever one receiver active at a time.
        private void SetPollTimer()
        {
            // Stop the timer if we are shutting down
            if (_shutdown)
                return;

            int wait = -1;
            // If the PollingInterval is negative, then polling is off
            if (_state.Configuration.PollingInterval >= 0 &&
                _state.Configuration.PollingInterval < _minimumPollingInterval)
                wait = _minimumPollingInterval;
            else
                wait = _state.Configuration.PollingInterval;

            if (wait >= 0)
                Activate(Arbiter.Receive(false, TimeoutPort(wait), PollingTimerHandler));
        }

        /// <summary>
        /// Reset the Timer after a timeout on a Poll
        /// </summary>
        /// <param name="dt"></param>
        private void SetPollTimer(DateTime dt)
        {
            SetPollTimer();
        }

        /// <summary>
        /// Polling Timer Handler
        /// </summary>
        /// <param name="time"></param>
        private void PollingTimerHandler(DateTime time)
        {
            // Stop the timer if we are shutting down
            if (_shutdown)
                return;

            // Ignore timer if not connected, but keep it ticking over
            if (!_state.Connected)
            {
                SetPollTimer();
                return;
            }

            // Is Polling turned off?
            if (_state.Configuration.PollingInterval < 0)
            {
                // Set the timer, but don't do anything
                SetPollTimer();
                return;
            }

            // Get Sensors
            Port<byte[]> response;
            response = _control.Poll();

            Activate(Arbiter.Choice(
                Arbiter.Receive<byte[]>(false, response, ProcessPollResult),
                // Make sure that we don't hang here forever!
                Arbiter.Receive<DateTime>(false, TimeoutPort(1000), SetPollTimer)));
        }

        /// <summary>
        /// Handle results from a Poll and reset Timer
        /// </summary>
        /// <param name="result"></param>
        private void ProcessPollResult(byte[] result)
        {
            // NOTE: The result should always be 3 bytes long or there is
            // something wrong
            if (result.Length == 3)
            {
                bool changed = false;

                // Process the response
                // Format should be Ixx
                if (result[0] == (byte)'I')
                {
                    // Convert the hex result to binary
                    int n = HexByteToInt(result, 1);
                    for (int i=0; i<_state.Sensors.Count; i++)
                    {
                        // Only process Digital sensors this way
                        if (util.IsDigitalDevice(_state.Sensors[i]))
                        {
                            // Construct a mask for this Pin
                            int mask = 1 << _state.Sensors[i].Pin;
                            bool oldState = _state.Sensors[i].State;
                            // Check if it is High or Low
                            if ((n & mask) != 0)
                            {
                                _state.Sensors[i].State = true;
                                _state.Sensors[i].Value = 1;
                            }
                            else
                            {
                                _state.Sensors[i].State = false;
                                _state.Sensors[i].Value = 0;
                            }
                            // Check for a state change
                            if (_state.Sensors[i].State != oldState)
                            {
                                // If it is the IR bumper, toggle an LED
                                // This is for debugging ...
                                if (_state.Sensors[i].Pin == (int)Pins.IrBumper)
                                {
                                    _state.LEDs = util.UpdateBitmask(_state.LEDs, LEDIds, (int)HwIds.LED2, _state.Sensors[i].State);
                                    _control.SetPin((int)Pins.LED2, _state.Sensors[i].State);
                                        
                                    //Console.WriteLine("IR Bumper: " + (n & mask));
                                }
                                changed = true;
                            }
                        }
                    }

                    // Only send notifications on changes
                    if (changed)
                        SendNotification<brick.UpdateSensors>(_subMgrPort, new brick.UpdateSensorsRequest(_state.Sensors));

                }
            }
            // Finally, set the timer again
            SetPollTimer();
        }

        #endregion

        #region Robot Connection

        /// <summary>
        /// Close the connection to the Robot
        /// </summary>
        private void Disconnect()
        {
            if (_state.Connected)
            {
                _control.Close();
                _state.Connected = false;
            }
        }

        /// <summary>
        /// Connect to the Robot
        /// </summary>
        /// <returns></returns>
        private bool ConnectToRobot()
        {
            string errorMessage;

            if (_state.Connected)
                return true;

            // Let user know what is happening
            // NOTE: Cannot use LogInfo here because this module is not derived
            // from the service base class!
            LogInfo(LogGroups.Console, "Connecting ...");

            // Attempt to connect to the robot
            _state.Connected = _control.Connect(_state.Configuration, out errorMessage);
            if (!_state.Connected && !string.IsNullOrEmpty(errorMessage))
            {
                // Display the error message and give up
                LogError(LogGroups.Activation, errorMessage);
                return false;
            }

            if (_control.WaitForConnect(_state.Firmware))
            {
                // Display the version for user to see
                LogInfo(LogGroups.Console, "Firmware Version: " + _state.Firmware.VersionString);

                // Indicate that connection was successful
                LogInfo(LogGroups.Console, "... Connected");

            }
            else
            {
                // Display an error message
                LogError(LogGroups.Activation, "Connection to robot failed on COM" + _state.Configuration.SerialPort + ".\nPlease fix the problem and re-start the service.\n");
                _state.Connected = false;
            }

            return _state.Connected;
        }

        #endregion

        #region Helper Functions

        int HexByteToInt(byte[] str, int offset)
        {
            int n;
            n = HexDigitToInt(str[offset]) * 16 + HexDigitToInt(str[offset+1]);
            return n;
        }

        int HexDigitToInt(byte d)
        {
            int n = 0;
            if (d >= (byte)'0' && d <= (byte)'9')
                n = d - (byte)'0';
            else if (d >= 'A' && d <= 'F')
                n = d - (byte)'A' + 10;
            else if (d >= 'a' && d <= 'f')
                n = d - (byte)'a' + 10;
            return n;
        }

        #endregion

    }

}
